#pragma once

#include <Eigen/Eigen>
#include "ParticlesData.h"
#include "MyMath.h"
#include "LogCat.h"
#include "Pose3D.h"
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include "MPointCloudMap.h"
#include <pcl/visualization/pcl_visualizer.h>
#include <tuple>
#define TAG "ParticleFilter"

typedef pcl::PointXYZ PointIoType;

struct InitPara
{
public:
	Eigen::Vector3d center;
	float start_x;
	float end_x;
	float start_y;
	float end_y;
	float start_z;
	float end_z;
	int particle_count;

	InitPara() {
	};

};

struct PF_option {
	float noisy = 1.0;
	int doParticleShift = 0;
	int shift_particle_function = 0;
	int show_shift_particles = 0;
	int particles_num = 200;
	float init_area_xy = 1.0;
	float init_area_z = 1.0;
};

struct ParticleFilterStats
{
	double ESS_beforeResample{ 0 };
	double weightsVariance_beforeResample{ 0 };
};
typedef void(*showPredictionParticles)(bool* has_add, 
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr show_particles,
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_pt);

class ParticleFilter:public ParticlesData,public MyMath,public LogCat
{
public:
	InitPara mInitPara;

	ParticleFilter();
	~ParticleFilter();
	void setCallback(showPredictionParticles showPredictionParticles,
		bool* has_add,
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr prediction_particles,
		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_pt);
	void predictionDone(ParticlesData::Particles& shift_particles);
	void executeOn(const Pose3D & action, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map);
	void prediction_and_update(const Pose3D& action, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map);
	void doResample();
	
	void MRPTRsample(vector<size_t>& out_indexes);
	void MRPTComputeResampling(const vector<double>& in_logWeights, vector<size_t>& out_indexes);
	void MRPTperformSubstitution(const std::vector<size_t>& indx);
	void normalizeWeights();
	void doParticleShift(const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map);
	void doParitcleShiftWithResampleIndex(vector<size_t>& indexes, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map);
	void cumsum_tmpl(vector<double>& in_data, vector<double>& out_cumsum);
	void particleDataInit(const vector<Pose3D>& ground_truth);
	void pfOptionInit(PF_option pf_option);
	void setParticle(int M);
	double ESS();
	

	void logOutPut(string root, vector<int> using_time, vector<Pose3D> pose_list);



	//bad function do not use
	void resample_Wheel();
	//bad function do not use
	vector<int> resample_Multinomial(const vector<double>& weights);
	

private:
	ParticleFilterStats PF_status;
	showPredictionParticles m_showPredictionParticles;
	bool* m_has_add;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr m_prediction_particles;
	boost::shared_ptr<pcl::visualization::PCLVisualizer> m_viewer_pt;
	PF_option mPF_option;

};

